﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MathCalculations
{
    //This function calculates the angle the missile launcher should shoot at in degrees.  
    //Z is the height, and X is the horizontal distance away the object is from the missile launcher.
    //This program assumes that the missile launcher is positioned at coordinate (0,0) for both x, and z.
    //This program will only give angles 0-90
    //X, and Y are given in meters
    public class AngleCalculation
    {
        private double initialVelocity;
        private double horizontalAngle;
        private double verticalAngle;
        public AngleCalculation()
        {
            initialVelocity = 53.67;
        }
        void setInitialVelocity(double velocity)
        {
            initialVelocity = velocity;
        }
        public double radiansToDegrees(double radians)
        {
            return radians * (180 / Math.PI);
        }
        public double degreesToRadians(double degrees)
        {
            return degrees * (Math.PI / 180);
        }
        public double cos(double degrees)
        {
            return Math.Cos(degreesToRadians(degrees));
        }
        public double sin(double degrees)
        {
            return Math.Sin(degreesToRadians(degrees));
        }
        public double getHorizontalAngle(double finalX, double finalZ)
        {
            horizontalAngle = radiansToDegrees(Math.Atan(finalX / finalZ));
            return horizontalAngle;
        }



        public double getVerticalAngle(double diffX, double diffY, double finalZ, double velocity = 53.67)
        {
            //verticalAngle = radiansToDegrees(Math.Atan(diffY / finalZ))*2;
            //return verticalAngle;
            double diffZ = Math.Sqrt(Math.Pow(diffX,2)+Math.Pow(finalZ,2));
            double rightSide;
            double leftSide;
            double difference;
            double bestDiff;
            double gravity = 386.22;
            initialVelocity = velocity;
            bestDiff = double.MaxValue;


            double distance = 0;
            double height = 0;

            for (int angle = 0; angle <= 90; angle++)
            {
                double ad = 0;
                double ah = 0;
                for (double time = 0.01; time < 3; time += .01)
                {
                    double d = time * initialVelocity * cos(angle);
                    if (Math.Abs(d - diffZ) < Math.Abs(ad - diffZ))
                    {
                        ad = d;
                    }

                    double h = time * initialVelocity * sin(angle) - .5 * gravity * Math.Pow(time, 2);
                    if (h < 0) { continue; }
                    if (Math.Abs(h - diffY) < Math.Abs(ah - diffY) && ad==d)
                    {
                        ah = h;
                    }

                    /*
                        //            Console.WriteLine("\n\n\n\n\n\n\n");
                    // Console.WriteLine("angle is {0}", angle);
                    //            Console.WriteLine("diffY {0}\n", diffY);
                    //            Console.WriteLine("initialVelocity {0}\n", initialVelocity);
                    //            Console.WriteLine("cos(angle) {0}\n", cos(angle));
                    leftSide = diffY * 2 * Math.Pow(initialVelocity, 2) * Math.Pow(cos(angle), 2);
                    //           Console.WriteLine("left side is {0}", leftSide);
                    rightSide = 2 * cos(angle) * sin(angle) * Math.Pow(initialVelocity, 2) * diffZ - gravity * Math.Pow(diffZ, 2);
                    //           Console.WriteLine("right side is {0}", rightSide);
                    difference = Math.Abs(leftSide - rightSide);
                    Console.WriteLine("difference is {0}", difference);

                    if (angle == 0)
                    {
                        bestDiff = difference;
                        verticalAngle = angle;
                    }
                    else if (difference < bestDiff)
                    {
                        bestDiff = difference;
                        verticalAngle = angle;
                    }
                    //            Console.WriteLine("best angle is {0}", verticalAngle);
                    //           Console.WriteLine("best difference is {0}", bestDiff);
                    //*/
                }
                if (Math.Abs(ah - diffY) < Math.Abs(height - diffY) && Math.Abs(ad - diffZ) < Math.Abs(distance - diffZ))
                {
                    distance = ad;
                    height = ah;
                    verticalAngle = angle;
                }

            }
            Console.WriteLine("returned angle is {0}", verticalAngle);
            return verticalAngle;
        }
    }
}
